//
// Created by zhangcheng on 2022/8/30.
//

#include "eigen3/Eigen/Dense"
#include "eigen3/Eigen/Core"
#include "guidance_filter/Polyfit.h"

Polyfit::Polyfit(int deg, int max_length) {
    order = deg;
    length = max_length;
    output = 0.0;
};


double Polyfit::polyeval(Eigen::VectorXd coeffs, double x) {
    double result = 0.0;
    for (int i = 0; i < coeffs.size(); i++) {result += coeffs[i]*pow(x, i);}
    return result;
}

Eigen::VectorXd Polyfit::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals) {
    assert(xvals.size() == yvals.size());
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::MatrixXd A(xvals.size(), order + 1);
    for (int i = 0; i < xvals.size(); i++) {
        A(i, 0) = 1.0;
    }
    for (int j = 0; j < xvals.size(); j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * xvals(j);
        }
    }
    auto Q = A.householderQr();
    auto result = Q.solve(yvals);
    return result;
}

bool Polyfit::Update(double input) {
    // Set Input To Eigen::VectorXd
    yvalds.push_back(input);
    if (yvalds.size() < order + 1){
        std::cout << "Amount of input data insufficient!!" << std::endl;
        return false;
    }
    if (yvalds.size() > length){
        std::vector<double>::iterator k = yvalds.begin();
        yvalds.erase(k);
    }

    int len = yvalds.size();
    Eigen::VectorXd xvals(len), yvals(len);
    for(int i=0; i < yvalds.size(); i++){
        xvals[i] =  (double)i;
        yvals[i] =  yvalds[i];
    }

    Eigen::VectorXd coeffs = this->polyfit(xvals, yvals);
    double result = this->polyeval(coeffs, (double)yvals.size() - 1);
    output = result;
    return true;
}
